/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "can.h"
#include "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "SEGGER_RTT.h"
#include "SEGGER_RTT_Conf.h"
#include "bsp.h"
#include <stdint.h>
#include <stdbool.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */

/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
MotorState motor_state = MOTOR_STOP; // 电机初始状态为停止
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
extern void SystemClock_Config(void);
extern void CAN_SendMessage(CAN_HandleTypeDef* hcan, uint32_t id, uint8_t *data, uint8_t length);
extern void Hall_signal_Init(void);
extern void LED_GPIO_Init(void);
extern void Return_to_zero_GPIO_Init(void);
extern void RTT_printf_float(unsigned BufferIndex, const char* pPrefix, float fValue, unsigned int Precision, const char* pSuffix);
extern void HoldMotorPosition(uint16_t holding_duty_percent);

unsigned char number = 0;
unsigned char starter_motor = 0;
unsigned char g_encoder_read_trigger = 0;
// g_motor_sport = 1 旋转
// g_motor_sport = 0 停止
// g_motor_sport = 2 堵转
unsigned char g_motor_sport = 0;
volatile int motor_direction = 0;   /* 电机旋转方向 */
volatile int capture_count = 0;     /* 当前时刻总计数值 */
volatile int last_count = 0;        /* 上一时刻总计数值 */
volatile int Encoder_Overflow_Count = 0;
volatile float motor_angle = 0.0f; 
volatile float last_motor_angle = -1.0f; // 用于存储上一次的角度值，初始化为一个不可能的值
volatile uint8_t hall_changed = 0;

unsigned char initial = 0;
uint8_t uc_motor_angle[8] = {0}; // 初始化为0

/// @brief 开环控制
/// @param  
void test_commutation(void) 
{
  for (int step = 0; step < 6; step++) 
  {
//      SetCommutationStep(step, 35);
			SetCommutationStep(step, 75);
      HAL_Delay(10); // 100毫秒延迟，观察旋转
      //SEGGER_RTT_CustomFunction(1, 0, "Test Step: %d, Time: %lu\n", step, HAL_GetTick());
  }
}

 /**
   * @brief  编码器接口引脚初始化
   * @param  无
   * @retval 无
   */
 static void Encoder_GPIO_Init(void)
 {
   GPIO_InitTypeDef GPIO_InitStruct = {0};

   /* 定时器通道引脚端口时钟使能 */
   ENCODER_TIM_CH1_GPIO_CLK_ENABLE();
   ENCODER_TIM_CH2_GPIO_CLK_ENABLE();

   /**TIM3 GPIO Configuration
   PC6     ------> TIM3_CH1
   PC7     ------> TIM3_CH2
   */
   /* 设置输入类型 */
   GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
   /* 设置上拉 */
   GPIO_InitStruct.Pull = GPIO_PULLUP;
   /* 设置引脚速率 */
   GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;

   /* 选择要控制的GPIO引脚 */
   GPIO_InitStruct.Pin = ENCODER_TIM_CH1_PIN;
   /* 设置复用 */
   GPIO_InitStruct.Alternate = ENCODER_TIM_CH1_GPIO_AF;
   /* 调用库函数，使用上面配置的GPIO_InitStructure初始化GPIO */
   HAL_GPIO_Init(ENCODER_TIM_CH1_GPIO_PORT, &GPIO_InitStruct);

   /* 选择要控制的GPIO引脚 */
   GPIO_InitStruct.Pin = ENCODER_TIM_CH2_PIN;
   /* 设置复用 */
   GPIO_InitStruct.Alternate = ENCODER_TIM_CH2_GPIO_AF;
   /* 调用库函数，使用上面配置的GPIO_InitStructure初始化GPIO */
   HAL_GPIO_Init(ENCODER_TIM_CH2_GPIO_PORT, &GPIO_InitStruct);
 }
 
/// @brief 
/// @param hcan 
/// @param id 
/// @param data 
/// @param length 
void CAN_SendMessage(CAN_HandleTypeDef* hcan, uint32_t id, uint8_t *data, uint8_t length)
{
    CAN_TxHeaderTypeDef TxHeader;
    uint32_t TxMailbox;

    TxHeader.StdId = id;              // 设置标准 CAN ID
    TxHeader.ExtId = 0;               // 扩展 ID 未使用
    TxHeader.IDE = CAN_ID_STD;        // 使用标准帧
    TxHeader.RTR = CAN_RTR_DATA;      // 数据帧
    TxHeader.DLC = length;            // 数据长度 (0-8 字节)
    TxHeader.TransmitGlobalTime = DISABLE;

    if (HAL_CAN_AddTxMessage(hcan, &TxHeader, data, &TxMailbox) != HAL_OK)
    {
        Error_Handler(); // 发送失败时调用错误处理
    }
}

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
	Return_to_zero_GPIO_Init();
	LED_GPIO_Init();
	Hall_signal_Init();
  //MX_DMA_Init();
  MX_TIM16_Init();
  MX_TIM15_Init();
  MX_TIM1_Init();
  MX_TIM3_Init();
  MX_CAN_Init();
  if (HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL) != HAL_OK)
  {
      Error_Handler();
  }
	HAL_TIM_Base_Start(&htim1);
	HAL_TIM_Base_Start(&htim15);
	HAL_GPIO_WritePin(NSLEEP_GPIO_Port, NSLEEP_Pin, GPIO_PIN_SET);
	SEGGER_RTT_ConfigUpBuffer(0, "RTTUP", NULL, 0, SEGGER_RTT_MODE_NO_BLOCK_SKIP);
	SEGGER_RTT_ConfigDownBuffer(0, "RTTDOWN", NULL, 0, SEGGER_RTT_MODE_NO_BLOCK_SKIP);
	SEGGER_RTT_SetTerminal(1);
	SEGGER_RTT_printf(0, "SEGGER_RTT_GetKey \r\n");	
	HAL_TIM_Base_Start_IT(&htim16);	

	unsigned char DipSwitch[4] = {0};
	DipSwitch[0] = HAL_GPIO_ReadPin(A0_Input_GPIO_Port, A0_Input_Pin);
	DipSwitch[1] = HAL_GPIO_ReadPin(A1_Input_GPIO_Port, A1_Input_Pin);
	DipSwitch[2] = HAL_GPIO_ReadPin(A2_Input_GPIO_Port, A2_Input_Pin);
	DipSwitch[3] = HAL_GPIO_ReadPin(A3_Input_GPIO_Port, A3_Input_Pin);
  number = DipSwitch[0] | DipSwitch[1] << 1 | DipSwitch[2] << 2 | DipSwitch[3] << 3;
  SEGGER_RTT_CustomFunction(1, 0, "canid_number = %d \r\n" , number);
	for(unsigned char i = 0 ; i < 4 ; i++)
		SEGGER_RTT_CustomFunction(1, 0, "DipSwitch[%d] = %d \r\n",i,DipSwitch[i]);
	
	motor_state = MOTOR_STOP;
  //ReadHallAndDetermineStep(); // 设置初始换向步骤
	
  #define TURN_ON_COMMUTATION
  while (1)
  {

		if(g_encoder_read_trigger == 1)
		{
			// 1. 获取电机方向
			motor_direction = __HAL_TIM_IS_TIM_COUNTING_DOWN(&htim3);    
			// 2. 计算当前的总计数值
			// 注意：如果不再需要连续追踪多圈的位置，可以简化，但保留下来更具通用性
			capture_count = __HAL_TIM_GET_COUNTER(&htim3) + (Encoder_Overflow_Count * ENCODER_TIM_PERIOD);
			// 3. 计算当前电机角度
			long long current_tick_in_rev = capture_count % ENCODER_TOTAL_RESOLUTION;
			if (current_tick_in_rev < 0) 
			{
					current_tick_in_rev += ENCODER_TOTAL_RESOLUTION;
			}
			motor_angle = (float)current_tick_in_rev / ENCODER_TOTAL_RESOLUTION * 360.0f; 
			// 4. 判断角度是否变化，如果变化则打印
			// 使用 fabsf 比较浮点数，避免精度问题导致的不必要打印
			// 阈值0.1度，可根据需要调整
			if (fabsf(motor_angle - last_motor_angle) > 0.1f) 
			{
					SEGGER_RTT_printf(0, "-----------------------\r\n");
					SEGGER_RTT_printf(0, "Motor direction: %s\r\n", motor_direction ? "Counter-Clockwise" : "Clockwise");
					RTT_printf_float(0, "Motor Angle: ", motor_angle, 4, " deg\r\n");
					last_motor_angle = motor_angle;
					/************************************************************/
					/*           新增：角度转换和CAN发送部分           */
					/************************************************************/
					
					// 1. 将浮点数角度转换为整数 (应用比例尺 1:0.01)
					//    乘以100并进行四舍五入，以提高精度
					uint16_t angle_as_int = (uint16_t)(motor_angle * 100.0f + 0.5f);
					// 2. 准备CAN发送的数据缓冲区
					// 3. 将16位整数拆分并存入数组 (低字节在前)
					uc_motor_angle[0] = (uint8_t)(angle_as_int & 0xFF);         // 低8位
					uc_motor_angle[1] = (uint8_t)((angle_as_int >> 8) & 0xFF);  // 高8位
					
					// (可选) 你可以在数组的其他字节中填充其他信息，例如方向
					uc_motor_angle[2] = motor_direction; // 0 for Clockwise, 1 for Counter-Clockwise
					
					// (可选) 为了调试，可以在RTT中打印将要发送的整数值和字节
					SEGGER_RTT_printf(0, "Sending Angle (int): %d, Bytes: [0x%02X, 0x%02X]\r\n", 
														angle_as_int, uc_motor_angle[0], uc_motor_angle[1]);

					// 4. 通过CAN总线发送数据
					//    这里假设你只想发送2个或3个字节的数据，所以长度设为3
					//    如果需要发送全部8字节，则长度为8
					//? 再这里发送数据非常不合理 , 已转移到定时器中断中发送
			}			
		}

		if(g_motor_sport == MOTOR_ROTATION)
		{
			if(initial == 1)
			{
        motor_state = MOTOR_ROTATION;
        initial = 0;
        ReadHallAndDetermineStep();
        SEGGER_RTT_SetTerminal(1);
        SEGGER_RTT_printf(0, "initial = %d \r\n" , initial);	
			}
      if (hall_changed) 
      {
        hall_changed = 0;
        #ifdef TURN_ON_COMMUTATION
        ReadHallAndDetermineStep();
        #else
        ReadHallAndDetermineStep_Demo();
        #endif
      }
	
		}
		else if(g_motor_sport == MOTOR_EMESTOP)
		{
      motor_state = MOTOR_EMESTOP;
      ReadHallAndDetermineStep();					
		}			
		else if(g_motor_sport == MOTOR_STALLED)	
		{//? 堵转
       motor_state = MOTOR_STALLED;
			ReadHallAndDetermineStep();			
		}
  }
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInit = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_BYPASS;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
  PeriphClkInit.PeriphClockSelection = RCC_PERIPHCLK_USART2|RCC_PERIPHCLK_TIM1;
  PeriphClkInit.Usart2ClockSelection = RCC_USART2CLKSOURCE_PCLK1;
  PeriphClkInit.Tim1ClockSelection = RCC_TIM1CLK_HCLK;
  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInit) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
